#include "dcMotor.h"

void dcMotor::setMotorControlPins( int logical_pin_1, int logical_pin_2, int pwm_pin )
{
	motor_control_pin_1 = logical_pin_1;
	motor_control_pin_2 = logical_pin_2;
	motor_pwm_pin = pwm_pin;

	//set control pins to produce output
	pinMode( motor_pwm_pin, OUTPUT);
	pinMode( motor_control_pin_1, OUTPUT);
	pinMode( motor_control_pin_2, OUTPUT);

	stopMotor();
}

void dcMotor::setMotorPower( int power)
{
	if( motor_power == power)
	{
		Serial.println( "not changing motor power" );
		return;
	}

	if( power == OFF_DUTY )
	{
		Serial.println( "motor stopped" );
		stopMotor();
	}
	else
	{
		motor_power = power;
		analogWrite( motor_pwm_pin, motor_power );
	}
}

int dcMotor::getMotorPower()
{
	return motor_power;
}

void dcMotor::setMotorDirection( int direction )
{
	switch( direction )
	{

		case FORWARD:
			Serial.println("what the hell is going on");
			digitalWrite( motor_control_pin_2, LOW );//avoid breaking
			digitalWrite( motor_control_pin_1, HIGH );
			break;
		case BACKWARD:
			digitalWrite( motor_control_pin_1, LOW );//avoid breaking
			digitalWrite( motor_control_pin_2, HIGH );
			break;
		default:
			Serial.print( "ERROR: unknown direction provided to setMotorDirection: " );
			Serial.println( direction );
			break;
	}
}

void dcMotor::stopMotor()
{
	motor_power = OFF_DUTY;
	analogWrite( motor_pwm_pin, 0 );
	digitalWrite( motor_control_pin_1, LOW );
	digitalWrite( motor_control_pin_2, LOW );
}

